#ifndef BTL_FEATURES_FEATUREMATCHER
#define BTL_FEATURES_FEATUREMATCHER

#include <btl/Maths/OnlineStatistics.hpp>
#include <Eigen/Dense>
#include <boost/tuple/tuple.hpp>
#include <boost/optional/optional.hpp>

#include <vector>
#include <map>
#include <functional>
#include <algorithm>
#include <iterator>

namespace btl
{
namespace features
{

// required interface of PredictedFeatureType:
// struct PredictedFeatureTypeInterface {
//    typedef XXX feature_descriptor_type;
//
//    Eigen::Vector2f position() const;
//    const feature_descriptor_type& descriptor() const;
// };

template <typename CandidateFeatureType>
struct FeatureSearcher
{
   typedef CandidateFeatureType candidate_type;
   typedef typename candidate_type::feature_descriptor_type descriptor_type;
};

template <typename CandidateIterType>
class FeatureSearcherFixedRadius :
   public FeatureSearcher<typename std::iterator_traits<CandidateIterType>::value_type>
{
   public:
      typedef CandidateIterType candidate_id;

      FeatureSearcherFixedRadius(CandidateIterType begin, CandidateIterType end, double radius);

      template <typename PredictedFeatureType, typename OutputIterType>
      void operator()(const PredictedFeatureType& predicted, OutputIterType output) const;

      CandidateIterType nullId() const;

   protected:
      CandidateIterType _begin;
      CandidateIterType _end;
      double _radius;
};

template <typename CandidateIterType>
FeatureSearcherFixedRadius<CandidateIterType> fixed_radius_searcher(
   CandidateIterType begin, CandidateIterType end, double radius);

template <typename PredictionIterType, typename OutputIterType, typename SearcherType, typename ScoreFunctor>
void married_matches(
   PredictionIterType begin, PredictionIterType end, OutputIterType output,
   const SearcherType& searcher, ScoreFunctor score_func, double min_score);

} //namespace features
} //namespace btl


namespace btl
{

using features::fixed_radius_searcher;
using features::married_matches;

} //namespace btl


// ====================================================================
// === Implementation


namespace btl
{
namespace features
{

// ~~~~~ Fixed Radius feature search ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

template <typename CandidateIterType>
inline FeatureSearcherFixedRadius<CandidateIterType>::FeatureSearcherFixedRadius(CandidateIterType begin, CandidateIterType end, double radius):
   _begin(begin), _end(end), _radius(radius) {}

template <typename CandidateIterType>
template <typename PredictedFeatureType, typename OutputIterType>
inline void FeatureSearcherFixedRadius<CandidateIterType>::operator()(
   const PredictedFeatureType& predicted, OutputIterType output) const
{
   Eigen::Vector2f targetPos = predicted.position();
   const double maxSquaredDist = _radius * _radius;
   for(CandidateIterType it = _begin; it != _end; ++it)
   {
      double squaredDist = (targetPos - it->position()).squaredNorm();
      if(squaredDist <= maxSquaredDist)
         *output++ = it;
   }
}

template <typename CandidateIterType>
inline CandidateIterType FeatureSearcherFixedRadius<CandidateIterType>::nullId() const
{
   return _end;
}

// helper function to deduce types for you if you're passing a
// FeatureSearcherFixedRadius straight to something else
template <typename CandidateIterType>
inline FeatureSearcherFixedRadius<CandidateIterType> fixed_radius_searcher(
   CandidateIterType begin, CandidateIterType end, double radius)
{
   return FeatureSearcherFixedRadius<CandidateIterType>(begin, end, radius);
}

// ~~~~~ Married Matching ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

template <typename PredictionIterType, typename OutputIterType, typename SearcherType, typename ScoreFunctor>
void married_matches(
   PredictionIterType begin, PredictionIterType end, OutputIterType output,
   const SearcherType& searcher, ScoreFunctor score_func, double min_score)
{
   typedef PredictionIterType prediction_id;
   typedef typename SearcherType::candidate_id candidate_id;

   typedef std::pair<double, prediction_id> candidate_match_score;
   typedef std::map<candidate_id, candidate_match_score> candidate_match_map;
   typedef std::pair<double, candidate_id> prediction_match_score;
   typedef std::vector<prediction_match_score> prediction_match_map;

   // store current best match in each direction
   candidate_match_map candidateMatchMap;
   prediction_match_map predictionMatchMap;

   btl::maths::OnlineStatistics scoreStats;
   btl::maths::OnlineStatistics matchSearchPoolStats;

   std::vector<candidate_id> selection;
   for(PredictionIterType predIt = begin; predIt != end; ++predIt)
   {
      predictionMatchMap.push_back(prediction_match_score(0.0, searcher.nullId()));
      prediction_match_score& bestCandidate = predictionMatchMap.back();

      selection.clear();
      searcher(*predIt, std::back_inserter(selection));

      matchSearchPoolStats.push(selection.size());

      for(typename std::vector<candidate_id>::const_iterator
            selIt = selection.begin(); selIt != selection.end(); ++selIt)
      {
         candidate_id candidateId = *selIt;

         candidate_match_score& bestPrediction = candidateMatchMap.insert(
               std::make_pair(candidateId, candidate_match_score(0.0, end))).first->second;

         double score = score_func(predIt->descriptor(), candidateId->descriptor());
         scoreStats.push(score);

         if(score > min_score)
         {
            if(score > bestCandidate.first)
               bestCandidate = prediction_match_score(score, candidateId);
            if(score > bestPrediction.first)
               bestPrediction = candidate_match_score(score, predIt);
         }
      }
   }

   typename prediction_match_map::iterator predMatchIt;
   PredictionIterType predIt;
   for(predIt = begin, predMatchIt = predictionMatchMap.begin();
         predIt != end; ++predIt, ++predMatchIt)
   {
      prediction_match_score& bestCandidate = *predMatchIt;

      if(bestCandidate.second != searcher.nullId())
      {
         typename candidate_match_map::iterator selMatchIt
         = candidateMatchMap.find(bestCandidate.second);
         assert(selMatchIt != candidateMatchMap.end());
         assert(selMatchIt->first == bestCandidate.second);
         if(selMatchIt->second.second == predIt)
            *output++ = std::make_pair(predIt, bestCandidate.second);
      }
   }
}

} //namespace features;
} //namespace btl


#endif //BTL_FEATURES_FEATUREMATCHER
